#include "maestro.h"
#include "compass.h"
#include "gotemp.h"
#include "gps_boat.h"
#include <gps.h>
#include <unistd.h>
#include "config.h"

int goToHeading (double desired_heading)
{
	usleep(5000);
	double ret_heading, ret_pitch, ret_roll, ret_temp;
	get_compass(&ret_heading, &ret_pitch, &ret_roll, &ret_temp);
	
	double difference = desired_heading - ret_heading;

	while(abs(difference) > 5)
	{
		if (difference < -180)
			difference = difference + 360;
		if (difference > 180)
			difference = difference - 360;

		unsigned int command = 1500 - 5*difference;
		fprintf(stderr, "Desired heading: %f, current heading: %f, Setting rudder pos: %d\n", desired_heading, ret_heading, command);

		//setTarget(maestro, rudder, command);
	}
	
	return 0;
}
